﻿#region Includes
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Runtime.InteropServices;
using System.Text;

using OpenTK;
using OpenTK.Graphics.OpenGL;
#endregion

namespace GingerGL.Math
{
    [Serializable]
    public class BoundingFrustum : Frustum, IEquatable<BoundingFrustum>
    {
        #region Variables
        protected Matrix4 mMatrix;
        protected Vector3[] mCorners;
        #endregion

        #region Properties
        /// <summary>
        /// Gets Frustum Matrix
        /// </summary>
        public Matrix4 Matrix
        {
            get { return this.mMatrix; }
            set
            {
                this.mMatrix = value;
                this.CreatePlanes();    // FIXME: The odds are the planes will be used a lot more often than the matrix
                this.CreateCorners();   // is updated, so this should help performance. I hope ;)
            }
        }
        #endregion

        #region Constructors
        /// <summary>
        /// Initializes a new instance of object from Matrix
        /// </summary>
        public BoundingFrustum(Matrix4 value)
        {
            this.mMatrix = value;
            CreatePlanes();
            CreateCorners();
        }
        #endregion

        #region Methods
        /// <summary>
        /// Create Frustum from Matrix
        /// </summary>
        protected void CreateCorners()
        {
            this.mCorners = new Vector3[8];
            this.mCorners[0] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Near], this.Planes[(int)FrustumPlanes.Left], this.Planes[(int)FrustumPlanes.Top]);
            this.mCorners[1] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Near], this.Planes[(int)FrustumPlanes.Right], this.Planes[(int)FrustumPlanes.Top]);
            this.mCorners[2] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Near], this.Planes[(int)FrustumPlanes.Right], this.Planes[(int)FrustumPlanes.Bottom]);
            this.mCorners[3] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Near], this.Planes[(int)FrustumPlanes.Left], this.Planes[(int)FrustumPlanes.Bottom]);
            this.mCorners[4] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Far], this.Planes[(int)FrustumPlanes.Left], this.Planes[(int)FrustumPlanes.Top]);
            this.mCorners[5] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Far], this.Planes[(int)FrustumPlanes.Right], this.Planes[(int)FrustumPlanes.Top]);
            this.mCorners[6] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Far], this.Planes[(int)FrustumPlanes.Right], this.Planes[(int)FrustumPlanes.Bottom]);
            this.mCorners[7] = Plane.IntersectionPoint(this.Planes[(int)FrustumPlanes.Far], this.Planes[(int)FrustumPlanes.Left], this.Planes[(int)FrustumPlanes.Bottom]);
        }
        /// <summary>
        /// Creates Planes from Matrix
        /// </summary>
        protected void CreatePlanes()
        {
            // Pre-calculate the different planes needed
            this.Planes[(int)FrustumPlanes.Left] = new Plane(-mMatrix.M14 - mMatrix.M11, -mMatrix.M24 - mMatrix.M21, -mMatrix.M34 - mMatrix.M31, -mMatrix.M44 - mMatrix.M41);
            this.Planes[(int)FrustumPlanes.Right] = new Plane(mMatrix.M11 - mMatrix.M14, mMatrix.M21 - mMatrix.M24, mMatrix.M31 - mMatrix.M34, mMatrix.M41 - mMatrix.M44);
            this.Planes[(int)FrustumPlanes.Top] = new Plane(mMatrix.M12 - mMatrix.M14, mMatrix.M22 - mMatrix.M24, mMatrix.M32 - mMatrix.M34, mMatrix.M42 - mMatrix.M44);
            this.Planes[(int)FrustumPlanes.Bottom] = new Plane(-mMatrix.M14 - mMatrix.M12, -mMatrix.M24 - mMatrix.M22, -mMatrix.M34 - mMatrix.M32, -mMatrix.M44 - mMatrix.M42);
            this.Planes[(int)FrustumPlanes.Near] = new Plane(-mMatrix.M13, -mMatrix.M23, -mMatrix.M33, -mMatrix.M43);
            this.Planes[(int)FrustumPlanes.Far] = new Plane(mMatrix.M13 - mMatrix.M14, mMatrix.M23 - mMatrix.M24, mMatrix.M33 - mMatrix.M34, mMatrix.M43 - mMatrix.M44);

            this.Planes[(int)FrustumPlanes.Left].Normalize();
            this.Planes[(int)FrustumPlanes.Right].Normalize();
            this.Planes[(int)FrustumPlanes.Top].Normalize();
            this.Planes[(int)FrustumPlanes.Bottom].Normalize();
            this.Planes[(int)FrustumPlanes.Near].Normalize();
            this.Planes[(int)FrustumPlanes.Far].Normalize();
        }
        public static bool operator ==(BoundingFrustum a, BoundingFrustum b)
        {
            if (object.Equals(a, null))
                return (object.Equals(b, null));

            if (object.Equals(b, null))
                return (object.Equals(a, null));

            return a.mMatrix == (b.mMatrix);
        }

        public static bool operator !=(BoundingFrustum a, BoundingFrustum b)
        {
            return !(a == b);
        }

        public ContainmentType Contains(BoundingBox box)
        {
            ContainmentType result;
            this.Contains(ref box, out result);
            return result;
        }

        public void Contains(ref BoundingBox box, out ContainmentType result)
        {
            // FIXME: Is this a bug?
            // If the bounding box is of W * D * H = 0, then return disjoint
            if (box.Min == box.Max)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            int i;
            ContainmentType contained;
            Vector3[] corners = box.GetCorners();

            // First we assume completely disjoint. So if we find a point that is contained, we break out of this loop
            for (i = 0; i < corners.Length; i++)
            {
                this.Contains(ref corners[i], out contained);
                if (contained != ContainmentType.Disjoint)
                    break;
            }

            if (i == corners.Length) // This means we checked all the corners and they were all disjoint
            {
                result = ContainmentType.Disjoint;
                return;
            }

            if (i != 0)             // if i is not equal to zero, we can fastpath and say that this box intersects
            {                       // because we know at least one point is outside and one is inside.
                result = ContainmentType.Intersects;
                return;
            }

            // If we get here, it means the first (and only) point we checked was actually contained in the frustum.
            // So we assume that all other points will also be contained. If one of the points is disjoint, we can
            // exit immediately saying that the result is Intersects
            i++;
            for (; i < corners.Length; i++)
            {
                this.Contains(ref corners[i], out contained);
                if (contained != ContainmentType.Contains)
                {
                    result = ContainmentType.Intersects;
                    return;
                }
            }

            // If we get here, then we know all the points were actually contained, therefore result is Contains
            result = ContainmentType.Contains;
            return;
        }

        // TODO: Implement this
        public ContainmentType Contains(BoundingFrustum frustum)
        {
            if (this == frustum)                // We check to see if the two frustums are equal
                return ContainmentType.Contains;// If they are, there's no need to go any further.

            throw new NotImplementedException();
        }

        public ContainmentType Contains(BoundingSphere sphere)
        {
            ContainmentType result;
            this.Contains(ref sphere, out result);
            return result;
        }

        public void Contains(ref BoundingSphere sphere, out ContainmentType result)
        {
            float val;
            ContainmentType contained;

            // We first check if the sphere is inside the frustum
            this.Contains(ref sphere.Center, out contained);

            // The sphere is inside. Now we need to check if it's fully contained or not
            // So we see if the perpendicular distance to each plane is less than or equal to the sphere's radius.
            // If the perpendicular distance is less, just return Intersects.
            if (contained == ContainmentType.Contains)
            {
                val = Plane.PerpendicularDistance(sphere.Center, this.Planes[(int)FrustumPlanes.Bottom]);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = Plane.PerpendicularDistance(sphere.Center, this.Planes[(int)FrustumPlanes.Far]);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = Plane.PerpendicularDistance(sphere.Center, this.Planes[(int)FrustumPlanes.Left]);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = Plane.PerpendicularDistance(sphere.Center, this.Planes[(int)FrustumPlanes.Near]);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = Plane.PerpendicularDistance(sphere.Center, this.Planes[(int)FrustumPlanes.Right]);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = Plane.PerpendicularDistance(sphere.Center, this.Planes[(int)FrustumPlanes.Top]);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                // If we get here, the sphere is fully contained
                result = ContainmentType.Contains;
                return;
            }
            //duff idea : test if all corner is in same side of a plane if yes and outside it is disjoint else intersect
            // issue is that we can have some times when really close aabb 



            // If we're here, the the sphere's centre was outside of the frustum. This makes things hard :(
            // We can't use perpendicular distance anymore. I'm not sure how to code this.
            throw new NotImplementedException();
        }

        public ContainmentType Contains(Vector3 point)
        {
            ContainmentType result;
            this.Contains(ref point, out result);
            return result;
        }

        public void Contains(ref Vector3 point, out ContainmentType result)
        {
            float val;
            // If a point is on the POSITIVE side of the plane, then the point is not contained within the frustum

            // Check the top
            val = Plane.ClassifyPoint(point, this.Planes[(int)FrustumPlanes.Top]);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the bottom
            val = Plane.ClassifyPoint(point, this.Planes[(int)FrustumPlanes.Bottom]);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the left
            val = Plane.ClassifyPoint(point, this.Planes[(int)FrustumPlanes.Left]);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the right
            val = Plane.ClassifyPoint(point, this.Planes[(int)FrustumPlanes.Right]);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the near
            val = Plane.ClassifyPoint(point, this.Planes[(int)FrustumPlanes.Near]);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the far
            val = Plane.ClassifyPoint(point, this.Planes[(int)FrustumPlanes.Far]);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // If we get here, it means that the point was on the correct side of each plane to be
            // contained. Therefore this point is contained
            result = ContainmentType.Contains;
        }

        public bool Equals(BoundingFrustum other)
        {
            return (this == other);
        }

        public override bool Equals(object obj)
        {
            BoundingFrustum f = obj as BoundingFrustum;
            return (object.Equals(f, null)) ? false : (this == f);
        }

        public Vector3[] GetCorners()
        {
            return mCorners;
        }

        public override int GetHashCode()
        {
            return this.mMatrix.GetHashCode();
        }

        public bool Intersects(BoundingBox box)
        {
            foreach (Plane plane in this.Planes)
            {
                if ((plane.Normal.X * box.Min.X) + (plane.Normal.Y * box.Min.Y) + (plane.Normal.Z * box.Min.Z) + plane.D > 0) continue;
                if ((plane.Normal.X * box.Max.X) + (plane.Normal.Y * box.Min.Y) + (plane.Normal.Z * box.Min.Z) + plane.D > 0) continue;
                if ((plane.Normal.X * box.Max.X) + (plane.Normal.Y * box.Min.Y) + (plane.Normal.Z * box.Max.Z) + plane.D > 0) continue;
                if ((plane.Normal.X * box.Min.X) + (plane.Normal.Y * box.Min.Y) + (plane.Normal.Z * box.Max.Z) + plane.D > 0) continue;
                if ((plane.Normal.X * box.Min.X) + (plane.Normal.Y * box.Max.Y) + (plane.Normal.Z * box.Min.Z) + plane.D > 0) continue; 
                if ((plane.Normal.X * box.Max.X) + (plane.Normal.Y * box.Max.Y) + (plane.Normal.Z * box.Min.Z) + plane.D > 0) continue;
                if ((plane.Normal.X * box.Max.X) + (plane.Normal.Y * box.Max.Y) + (plane.Normal.Z * box.Max.Z) + plane.D > 0) continue;
                if ((plane.Normal.X * box.Min.X) + (plane.Normal.Y * box.Max.Y) + (plane.Normal.Z * box.Max.Z) + plane.D > 0) continue;

                // all points are behind the one plane so they can't be inside any other plane
                return false;
            }

            return true;
        }

        public bool Intersects(IEnumerable<Vector3> polygon)
        {
            foreach (Plane plane in this.Planes)
            {
                bool gotOne = false;
                foreach (Vector3 vert in polygon)
                {
                    if ((plane.Normal.X * vert.X) + (plane.Normal.Y * vert.Y) + (plane.Normal.Z * vert.Z) + plane.D > 0)
                    {
                        gotOne = true;
                        break;
                    }
                }
                if (gotOne)
                    continue;

                // all points are behind the one plane so they can't be inside any other plane
                return false;
            }
            return true;
        }

        public void Intersects(ref BoundingBox box, out bool result)
        {
            result = Intersects(box);
        }

        public void Intersects(ref IEnumerable<Vector3> polygon, out bool result)
        {
            result = Intersects(polygon);
        }

        public bool Intersects(BoundingFrustum frustum)
        {
            return Contains(frustum) != ContainmentType.Disjoint;
        }

        public bool Intersects(BoundingSphere sphere)
        {
            return Contains(sphere) != ContainmentType.Disjoint;
        }

        public void Intersects(ref BoundingSphere sphere, out bool result)
        {
            result = Contains(sphere) != ContainmentType.Disjoint;
        }

        public PlaneIntersectionType Intersects(Plane plane)
        {
            throw new NotImplementedException();
        }

        public void Intersects(ref Plane plane, out PlaneIntersectionType result)
        {
            throw new NotImplementedException();
        }

        public Nullable<float> Intersects(Ray ray)
        {
            throw new NotImplementedException();
        }

        public void Intersects(ref Ray ray, out Nullable<float> result)
        {
            throw new NotImplementedException();
        }

        public override string ToString()
        {
            StringBuilder sb = new StringBuilder(256);
            sb.Append("{Near:");
            sb.Append(this.Planes[(int)FrustumPlanes.Near].ToString());
            sb.Append(" Far:");
            sb.Append(this.Planes[(int)FrustumPlanes.Far].ToString());
            sb.Append(" Left:");
            sb.Append(this.Planes[(int)FrustumPlanes.Left].ToString());
            sb.Append(" Right:");
            sb.Append(this.Planes[(int)FrustumPlanes.Right].ToString());
            sb.Append(" Top:");
            sb.Append(this.Planes[(int)FrustumPlanes.Top].ToString());
            sb.Append(" Bottom:");
            sb.Append(this.Planes[(int)FrustumPlanes.Bottom].ToString());
            sb.Append("}");
            return sb.ToString();
        }

        #endregion Public Methods
    }
}